#ifndef GAMEENGINE_AI_PATH_FINDING_ASTAR_HPP
#define GAMEENGINE_AI_PATH_FINDING_ASTAR_HPP



#ifdef GAMEENGINE_NAMESPACES
namespace GameEngine {
#endif

//! Find a path in a graph with the A-star algorithm
/*!
 @param res Receives the list of nodes that define the shortest path (can be empty if no path joins the two nodes)
 @param graph The graph that holds the nodes
 @param start The node to start with
 @param end The node to reach
 @param approximate_path If true, allows to select a path that does not actually go to node end (if no path
                         actually connects node start to node end). The path then goes from node start to the
						 node of the graph that is the closest to node end AND is connected to node start.
 @return True if the shortest path could be found, false otherwise
 */
template <class Graph, class Heuristic>
bool AStar<Graph,Heuristic>::find_path(
	std::list<typename Graph::Node>& res,
	const Graph& graph, const typename Graph::Node& start, const typename Graph::Node& end,
	bool approximate_path)
{
	Heuristic heuristic;
	return find_path(res, graph, start, end, heuristic, approximate_path);
}

//! Find a path in a graph with the A-star algorithm
/*!
 @param res Receives the list of nodes that define the shortest path (can be empty if no path joins the two nodes)
 @param graph The graph that holds the nodes
 @param start The node to start with
 @param end The node to reach
 @param heuristic The heuristic used to favor the most promising nodes in the research (speeds up the algorithm)
 @param approximate_path If true, allows to select a path that does not actually go to node end (if no path
                         actually connects node start to node end). The path then goes from node start to the
						 node of the graph that is the closest to node end AND is connected to node start.
 @return True if the shortest path could be found, false otherwise
 */
template <class Graph, class Heuristic>
bool AStar<Graph,Heuristic>::find_path(
	std::list<typename Graph::Node>& res,
	const Graph& graph, const typename Graph::Node& start, const typename Graph::Node& end,
	const Heuristic& heuristic, bool approximate_path)
{
	// Check data
	if ( end == start ) return false;

	// Try to find a path using the A-star algorithm
	AStar<Graph,Heuristic> search;
	search.find_path_internal(graph, start, end, heuristic);

	// Rebuild this path
	AStarNode<Graph> current;
	bool test = search.find_close_node(end, &current);
	if ( test == false ) { // There does not exist a path that connects the two nodes
		// Approximate paths are not acceptable
		if ( approximate_path == false ) return false;

		// Approximate paths are OK
		// Find the closest point to node end
		AStarList::const_iterator it = search.closed_list_.begin();
		current = *it; ++it;
		typename Graph::Cost cost = heuristic.cost(current.node(), end);
		for ( ; it != search.closed_list_.end(); ++it ) {
			typename Graph::Cost tmp_cost = heuristic.cost(it->node(), end);
			if ( tmp_cost >= cost ) continue;
			current = *it;
			cost = tmp_cost;
		}
	}

	res.push_front(current.node());
	while ( current.parent() != start ) {
		res.push_front(current.parent());
		if ( search.find_close_node(current.parent(), &current) == false ) break;
	}
	res.push_front(start);
	return true;
}

//! Default constructor
template <class Graph, class Heuristic>
AStar<Graph,Heuristic>::AStar() {}

//! Destructor
template <class Graph, class Heuristic>
AStar<Graph,Heuristic>::~AStar() {}

//! Try to find a path from the start node to the end node
/*!
 @param graph The graph that holds the nodes
 @param start The node to start with
 @param end The node to reach
 @param heuristic The heuristic used to favor the most promising nodes in the research (speeds up the algorithm)
 @return True if a path could be found, false otherwise
 */
template <class Graph, class Heuristic>
bool AStar<Graph,Heuristic>::find_path_internal(const Graph& graph,
												const typename Graph::Node& start,
												const typename Graph::Node& end,
												const Heuristic& heuristic)
{
	// Clear open and closed lists
	open_list_.clear();
	closed_list_.clear();

	// Initialization of the A-star algorithm
	AStarNode<Graph> last(start, start, 0);
	open_list_.insert( last );

	// Iteration of A-star
	while ( last.node() != end ) {
		// Check if there are still nodes to consider in the open list
		if ( open_list_.empty() ) {
			return false;
		}

		// Get lower cost node in the open list
		pick_node(last, end, heuristic);

		// Add this node to the closed list
		close_node(last);

		// Terminate path if required
		if ( last.node() == end ) {
			break;
		}

		// Get the children of this node and add them to the open list
		std::list<typename Graph::Node> children;
		graph.get_children(children, last.node());
		for ( std::list<typename Graph::Node>::iterator it = children.begin(); it != children.end(); ++it ) {
			// Check that this is not the parent node
			if ( *it == last.parent() ) continue;

			// Compute the cost to this node
			typename Graph::Cost cost = graph.cost(last.node(), *it);

			// Try to add this node to the open list
			open_node( *it, last.node(), last.cost()+cost );
		}
	}

	// End of the A-star algorithm
	return true;
}

//! Take the node in the open list whose total cost (previous cost + heuristic cost) is the lower
/*!
 *  The returned node is removed from the open list
 *
 @note The cost of returned node does NOT include cost estimated by the heuristic
 @param res Receives the node in the open list whose total cost is the lower
 @param end The node towards which we are heading
 @param heuristic The heuristic used to favor the most promising nodes in the research (speeds up the algorithm)
 @return Always return true
 */
template <class Graph, class Heuristic>
bool AStar<Graph,Heuristic>::pick_node(AStarNode<Graph>& res, const typename Graph::Node& end, const Heuristic& heuristic) {
	// Get the cost to the first node
	AStarList::iterator it = open_list_.begin();
	AStarList::iterator it_best = it;
	typename Graph::Cost cost_best = it->cost() + heuristic.cost(it->node(), end);
	++it;

	// Iterate over the nodes of the open list to look for the best
	for ( ; it != open_list_.end(); ++it ) {
		// Compute the cost of the current node
		typename Graph::Cost cost = it->cost() + heuristic.cost(it->node(), end);

		// Compare with the best node
		if ( cost < cost_best ) {
			it_best = it;
			cost_best = cost;
		}
	}

	// Remove the best node
	res = *it_best;
	open_list_.erase(it_best);
	return true;
}

//! Add the node to the closed list
template <class Graph, class Heuristic>
void AStar<Graph,Heuristic>::close_node(const AStarNode<Graph>& node) {
	closed_list_.insert(node);
}

//! Add the node to the open list, if required
/*!
 *  Three different cases are possible:<br>
 *  1. If node is already present in the closed list, it is not added<br>
 *  2. If node is already present in the open list, node is replaced if the cost is lower<br>
 *  3. Otherwise, node is added
 *
 @param node The node to add
 @param parent The parent of the node
 @param cost The cost to reach the node
 */
template <class Graph, class Heuristic>
void AStar<Graph,Heuristic>::open_node(const typename Graph::Node& node, const typename Graph::Node& parent, const typename Graph::Cost& cost) {
	// Check if node is not already in the close list
	if ( find_close_node(node) == true ) return;

	// Check if node is not already in the open list
	AStarNode<Graph> open_node(node, parent, cost);
	if ( find_open_node(node, &open_node) == true ) {
		if ( cost < open_node.cost() ) {
			AStarList::iterator it = open_list_.find(open_node);
			it->parent() = parent;
			it->cost() = cost;
		}
		return;
	}

	// Add the node to the open list
	open_list_.insert(open_node);
}

//! Find if given node is present in the closed list
/*!
 @param node The node to find in the list
 @param astarnode If not nil and if the node is present in the list, its value will be copied in the astarnode variable
 @return True if the node was found in the closed list, false otherwise
 */
template <class Graph, class Heuristic>
bool AStar<Graph,Heuristic>::find_close_node(const typename Graph::Node& node, AStarNode<Graph>* astarnode) const {
	AStarNode<Graph> tmp; tmp.node() = node;
	AStarList::const_iterator it = closed_list_.find(tmp);
	if ( it == closed_list_.end() ) return false;
	if ( astarnode != nil ) *astarnode = *it;
	return true;
}

//! Find if given node is present in the open list
/*!
 @param node The node to find in the list
 @param astarnode If not nil and if the node is present in the list, its value will be copied in the astarnode variable
 @return True if the node was found in the open list, false otherwise
 */
template <class Graph, class Heuristic>
bool AStar<Graph,Heuristic>::find_open_node(const typename Graph::Node& node, AStarNode<Graph>* astarnode) const {
	AStarNode<Graph> tmp; tmp.node() = node;
	AStarList::const_iterator it = open_list_.find(tmp);
	if ( it == open_list_.end() ) return false;
	if ( astarnode != nil ) *astarnode = *it;
	return true;
}


#ifdef GAMEENGINE_NAMESPACES
} // namespace GameEngine
#endif

#endif
